#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define NODE_NUM_AC 18
#define BRANCH_NUM_AC 36
#define NODE_NUM_DC 4
#define BRANCH_NUM_DC 9

template <typename Derived1, typename Derived2, typename Derived3>
void matrixSub(int m, int n, const MatrixBase<Derived1> &A, const MatrixBase<Derived2> &B, MatrixBase<Derived3> &C)
{
    C.setZero();
    
    // Perform matrix multiplication
    for (int i = 0; i < m; i++) 
    {
        for (int j = 0; j < n; j++) 
        {
            C(i, j) = A(i, j) - B(i, j);
        }
    }
}

//A <m, k>   B <k, n>   C<m, n>
template <typename Derived1, typename Derived2, typename Derived3>
void matrixMultiply(int m, int n, int k, const MatrixBase<Derived1> &A, const MatrixBase<Derived2> &B, MatrixBase<Derived3> &C)
{
    C.setZero();
    
    // Perform matrix multiplication
    for (int i = 0; i < m; i++) 
    {
        for (int j = 0; j < n; j++) 
        {
            for (int l = 0; l < k; l++) 
            {
                C(i, j) += A(i, l) * B(l, j);
            }
        }
    }
}

template <typename Derived1, typename Derived2>
void matrixTranspose(int rows, int cols, const MatrixBase<Derived1> &A, MatrixBase<Derived2> &B)
{
    for(int i = 0; i < rows; i++)
    {
        for(int j = 0; j < cols; j++)
        {
            B(j, i) = A(i, j);
        }
    }
}

template <typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6>
void matrixPartitioning_TOTAL(const MatrixBase<Derived1> &Y, const MatrixBase<Derived2> &J, MatrixBase<Derived3> &Y1, MatrixBase<Derived4> &Y2, MatrixBase<Derived5> &J1, MatrixBase<Derived6> &J2)
{
    //Y1
    for (int i = 0; i < NODE_NUM_AC; i++) 
    {
        for (int j = 0; j < NODE_NUM_AC; j++) 
        {
            Y1(i, j) = Y(i, j);
        }
    }

    //Y2
    for (int i = 0; i < NODE_NUM_DC; i++) 
    {
        for (int j = 0; j < NODE_NUM_DC; j++) 
        {
            Y2(i, j) = Y(i + NODE_NUM_AC, j + NODE_NUM_AC);
        }
    }

	//J1
	for(int i = 0; i < NODE_NUM_AC; i++)
	{
		J1(i) = J(i);
	}

	//J2
	for(int i = 0; i < NODE_NUM_DC; i++)
	{
		J2(i) = J(i + NODE_NUM_AC);
	}

}

template <typename Derived1, typename Derived2, typename Derived3, typename Derived4, typename Derived5, typename Derived6, typename Derived7, typename Derived8>
void matrixPartitioning_ACside(const MatrixBase<Derived1> &Y, const MatrixBase<Derived2> &J, MatrixBase<Derived3> &Y11, MatrixBase<Derived4> &Y12, MatrixBase<Derived5> &Y21, MatrixBase<Derived6> &Y22, MatrixBase<Derived7> &Jex, MatrixBase<Derived8> &Jin)
{
    //Y11
    for (int i = 0; i < 3; i++) 
    {
        for (int j = 0; j < 3; j++) 
        {
            Y11(i, j) = Y(i, j);
        }
    }

    //Y12
    for (int i = 0; i < 3; i++) 
    {
        for (int j = 0; j < NODE_NUM_AC - 3; j++) 
        {
            Y12(i, j) = Y(i, j + 3);
        }
    }

    //Y21
    for (int i = 0; i < NODE_NUM_AC - 3; i++) 
    {
        for (int j = 0; j < 3; j++) 
        {
            Y21(i, j) = Y(i + 3, j);
        }
    }

    //Y22
    for (int i = 0; i < NODE_NUM_AC - 3; i++) 
    {
        for (int j = 0; j < NODE_NUM_AC - 3; j++) 
        {
            Y22(i, j) = Y(i + 3, j + 3);
        }
    }

	//Jex
	for(int i = 0; i < 3; i++)
	{
		Jex(i) = J(i);
	}

	//Jin
	for(int i = 0; i < NODE_NUM_AC - 3; i++)
	{
		Jin(i) = J(i + 3);
	}

}

template <typename Derived>
void matrixInverse_LU(const MatrixBase<Derived> &A, MatrixBase<Derived> &A_inv, int N)
{
    //double W[N][N], W_n[N][N], L[N][N], U[N][N], L_n[N][N], U_n[N][N];
	int i, j, k, d;
	double s;

	double** W = new double*[N];
    for (int i = 0; i < N; i++) {
        W[i] = new double[N];
    }

	double** W_n = new double*[N];
    for (int i = 0; i < N; i++) {
        W_n[i] = new double[N];
    }

	double** L = new double*[N];
    for (int i = 0; i < N; i++) {
    	L[i] = new double[N];
    }

	double** U = new double*[N];
    for (int i = 0; i < N; i++) {
        U[i] = new double[N];
    }

	double** L_n = new double*[N];
    for (int i = 0; i < N; i++) {
        L_n[i] = new double[N];
    }

	double** U_n = new double*[N];
    for (int i = 0; i < N; i++) {
        U_n[i] = new double[N];
    }

 
	// 赋初值
	for(i=0;i<N;i++){
		for(j=0;j<N;j++){
			W[i][j] = A(i, j);
			L[i][j] = 0;
			U[i][j] = 0;
			L_n[i][j] = 0;
			U_n[i][j] = 0;
			W_n[i][j] = 0;
		}
	}
 
	for(i=0;i<N;i++)  // L对角置1
	{
		L[i][i] = 1.0;
	}
 
	for(j=0;j<N;j++)  
	{
		U[0][j] = W[0][j];
	}
 
	for(i=1;i<N;i++)
	{
		L[i][0] = W[i][0] / U[0][0];
	}
 
	for(i=1;i<N;i++)
	{
		for(j=i;j<N;j++) // 求U
		{
			s = 0;
			for(k=0;k<i;k++)
			{
				s += L[i][k] * U[k][j];
			}
			U[i][j] = W[i][j] - s;
		}
 
		for(d=i;d<N;d++) // 求L
		{
			s = 0;
			for(k=0;k<i;k++)
			{
				s += L[d][k] * U[k][i];
			}
			L[d][i] = (W[d][i] - s) / U[i][i];
		}
	}
 
	for(j=0;j<N;j++)  //求L的逆
	{
		for(i=j;i<N;i++)
		{
			if(i==j) 
				L_n[i][j] = 1 / L[i][j];
			else if(i<j) 
				L_n[i][j] = 0;
			else
			{
				s = 0.;
				for(k=j;k<i;k++)
				{
					s += L[i][k] * L_n[k][j];
				}
				L_n[i][j] = -L_n[j][j] * s;
			}
		}
	}
 
	for(i=0;i<N;i++)  //求U的逆
	{
		for(j=i;j>=0;j--)
		{
			if(i==j)
				U_n[j][i] = 1 / U[j][i];
			else if(j>i) 
				U_n[j][i] = 0;
			else
			{
				s = 0.;
				for(k=j+1;k<=i;k++)
				{
					s += U[j][k] * U_n[k][i];
				}
				U_n[j][i] = -1 / U[j][j] * s;
			}
		}
	}
 
 
	for(i=0;i<N;i++)
	{
		for(j=0;j<N;j++)
		{
			for(k=0;k<N;k++)
			{
				W_n[i][j] += U_n[i][k] * L_n[k][j];
			}
		}
	}

    for(i = 0; i < N; i++)
    {
        for(j = 0; j < N; j++)
        {
            A_inv(i, j) = W_n[i][j];
        }
    }

	for (int i = 0; i < N; i++) 
	{
    	delete[] W[i];
	}
	delete[] W;

	for (int i = 0; i < N; i++) 
	{
    	delete[] W_n[i];
	}
	delete[] W_n;

	for (int i = 0; i < N; i++) 
	{
    	delete[] L[i];
	}
	delete[] L;

	for (int i = 0; i < N; i++) 
	{
    	delete[] U[i];
	}
	delete[] U;

	for (int i = 0; i < N; i++) 
	{
    	delete[] L_n[i];
	}
	delete[] L_n;

	for (int i = 0; i < N; i++) 
	{
    	delete[] U_n[i];
	}
	delete[] U_n;

}

// 高斯消元法求矩阵逆
//使用高斯消元法对矩阵进行求逆
template <typename Derived>
void matrixInverse_Gauss(const MatrixBase<Derived> &A, MatrixBase<Derived> &A_inv)
{
	int i, j, k;
	double W[N][2*N], result[N][N];
	double tem_1, tem_2, tem_3;
 
	// 对矩阵右半部分进行扩增
	for(i = 0; i < N; i++)
    {
		for(j = 0; j < 2 * N; j++)
        {
			if(j<N)
            {
				W[i][j] = A(i, j);
			}
			else
            {
				W[i][j] = (double) (j-N == i ? 1:0);
			}
		}
	}
 
	for(i=0;i<N;i++)
	{
		// 判断矩阵第一行第一列的元素是否为0，若为0，继续判断第二行第一列元素，直到不为0，将其加到第一行
		if( ((int) W[i][i]) == 0)
		{ 
			for(j=i+1;j<N;j++)
			{
				if( ((int) W[j][i]) != 0 ) break;
			}
			if(j == N)
			{
				printf("Can not inverse!");
				break;
			}
			//将前面为0的行加上后面某一行
			for(k=0;k<2*N;k++)
			{
				W[i][k] += W[j][k];
			}
		}
 
		//将前面行首位元素置1
		tem_1 = W[i][i];
		for(j=0;j<2*N;j++)
		{
			W[i][j] = W[i][j] / tem_1;
		}
 
		//将后面所有行首位元素置为0
		for(j=i+1;j<N;j++)
		{
			tem_2 = W[j][i];
			for(k=i;k<2*N;k++)
			{
				W[j][k] = W[j][k] - tem_2 * W[i][k];
			}
		}
	}
 
	// 将矩阵前半部分标准化
	for(i=N-1;i>=0;i--)
	{
		for(j=i-1;j>=0;j--)
		{
			tem_3 = W[j][i];
			for(k=i;k<2*N;k++)
			{
				W[j][k] = W[j][k] - tem_3*W[i][k];
			}
		}
	}
 
	//得出逆矩阵
	for(i=0;i<N;i++)
	{
		for(j=N;j<2*N;j++)
		{
			result[i][j-N] = W[i][j];
		}
	}
 
	//printf("使用高斯消元法矩阵求逆的结果为：\n");
	for(i = 0; i < N; i++)
    {
        for(j = 0; j < N; j++)
        {
            A_inv(i, j) = result[i][j];
        }
    }
 	
}